package com.testcolision.deprecated;

import com.badlogic.gdx.math.Matrix4;
import com.badlogic.gdx.math.Vector3;
import com.badlogic.gdx.math.collision.BoundingBox;

public class ConstraintSphere {

	public static final float MAX_VEL = 200f;
	public static final float ACCELERATION = 30f;
	public static final float DAMP = 0.96f;

	static final int STATE_ACCLERATING = 0;
	static final int STATE_IDLE = 1;

	static final int DIRECTION_FORWARD = 1;
	static final int DIRECTION_REVERSE = -1;

	public Vector3 pos = new Vector3();

	public float accel = 0;

	public float angle = 0;
	public float speed, lastSpeed;
	public Vector3 vel = new Vector3(0, 0, 0);
	public float gravity = 0f;
	public float steerAngle = 0;
	public Matrix4 rotation = new Matrix4();
	int state = STATE_IDLE;
 
	public BoundingBox carBox;
	private float angleAccel;

	public ConstraintSphere(Vector3 pos) {
		this.pos.set(pos); 

	}

	public void update(float delta, Vector3 directionVec, Vector3 upVec) {

		speed += accel * delta;
		angle += angleAccel *delta;
		vel.set(directionVec).nor().mul(speed);
//		 vel.x = (float) Math.sin(angle * Math.PI / 180) * speed;
//		 vel.z = (float) Math.cos(angle * Math.PI / 180) * -speed;
	rotation.setToRotation(upVec, angle);
 	vel.rot(rotation);
		pos.add(vel.x * delta, 0, vel.z * delta);

		// pos.add(speedX * delta, 0, speedY * delta);
		if (speed >= MAX_VEL) {
			speed = MAX_VEL;
		}
		if (angle > 45) {
			angle = 45;
		} else if (angle < -45) {
			angle = -45;
		}

	
		speed *= DAMP;
		accel *= DAMP;
		angle *= DAMP;
		angleAccel*=DAMP;
	}

	public void accelerate(float delta, int dir) {

		accel = ACCELERATION * dir;

	}

	public void steerLeft(float delta) {
//		if ((speed < 2f && speed > 0) || (speed > -2f && speed < 0))
//			angle += 3f * Math.abs(speed) * direction;
//		else
			angleAccel  = 10f ;
	}

	public void steerRight(float delta) {
//		if ((speed < 2f && speed > 0) || (speed > -2f && speed < 0))
//			angle -= 3f * Math.abs(speed) * direction;
//		else
			angleAccel = -10f;
	}
}
